#include <iostream> 
#include "cooperate_locate.h"
#include "common.h"

const double CAMERA_OFFSET = 0; // 摄像头在车前方0.5米

// 在车上添加相机的相对位置偏移
void applyCameraOffset(Vehicle &car, double theta) {
    double dx = CAMERA_OFFSET * cos(theta);
    double dy = CAMERA_OFFSET * sin(theta);
    car.x += dx;
    car.y += dy;
}

// 将车的ned坐标系转换成enu坐标系
double ned_to_enu_angle(double theta_ned) {
    return M_PI_2 - theta_ned;
}

Vector3d get_obj_enu_from_car(VehicleWgs84 &car1_wgs84, VehicleWgs84 &car2_wgs84,  bool& flag)
{
    // 以car1的经纬度为基准建立坐标系
    ENUConverter converter(car1_wgs84.longitude, car1_wgs84.latitude);

    // 转换航向角从NED到ENU
    double theta1_enu = ned_to_enu_angle(car1_wgs84.theta);
    double theta2_enu = ned_to_enu_angle(car2_wgs84.theta);

    // 计算car1和car2在坐标系中的坐标
    Vehicle car1 = {0, 0, theta1_enu, -car1_wgs84.alpha};
    Vehicle car2 = {0, 0, theta2_enu, -car2_wgs84.alpha};
    std::cout << std::fixed << std::setprecision(4);
    std::cout << "car1 theta and alpha:(" << radiansToDegrees(car1.theta) << ", " << radiansToDegrees(car1.alpha) << ")"
              << std::endl;
    std::cout << std::fixed << std::setprecision(4);
    std::cout << "car2 theta and alpha:(" << radiansToDegrees(car2.theta) << ", " << radiansToDegrees(car2.alpha) << ")"
              << std::endl;
    
    // 将车的位置移动到摄像头位置（车前0.5米）
    applyCameraOffset(car1, theta1_enu);

    std::cout << std::fixed << std::setprecision(2);
    std::cout << "car1 enu:(" << car1.x << ", " << car1.y << ")"
              << std::endl;

    converter.Forward(car2_wgs84.longitude, car2_wgs84.latitude, car2.x, car2.y);
    applyCameraOffset(car2, theta2_enu);
    std::cout << std::fixed << std::setprecision(2);
    std::cout << "car2 enu:(" << car2.x << ", " << car2.y << ")"
              << std::endl;

    Vector3d obj;
    if (calculateObjectPosition(car1, car2, obj.x, obj.y))
        std::cout << "obj enu from car:(" << obj.x << ", " << obj.y << ")"
                  << std::endl;
    else{
        std::cout << "car ERROR" << std::endl;
        flag = false;
    }
        
    return obj;
}

Vector3d get_obj_enu_from_drone(Vector3d &drone_wgs84, Vector3d &camera_pos,
                                Vector3d &pixel, Matrix3x3 &drone_rot, Matrix3x3 &camera_rot,  bool& flag)
{
    // 以无人机的经纬度为基准建立坐标系，忽略海拔
    ENUConverter converter(drone_wgs84.x, drone_wgs84.y);
    // 无人机状态, 应用相机相对地面的修正
    Vector3d drone_enu(0, 0, drone_wgs84.z + 0.38);

    // 设置相机内参
    drone_coord::CameraIntrinsics intrinsics;

    Vector3d global_pos = drone_coord::get_global_pos(drone_enu, camera_pos, drone_rot);
    // 坐标转换
    Vector3d obj;
    Vector3d ray_drone; // 用于存储射线方向
    double t;           // 用于存储计算得到的t值
    if (drone_coord::pixel_to_global(pixel, global_pos, drone_rot, camera_rot, intrinsics, obj, ray_drone, t)){
        // 调试使用
        // std::cout << "射线方向: (" << ray_drone.x << ", " << ray_drone.y << ", " << ray_drone.z << ")" << std::endl;
        // std::cout << "t值: " << t << std::endl;
        std::cout << "obj enu from drone: (" << obj.x << ", " << obj.y << ")" << std::endl;
    }else{
        std::cout << "drone ERROR" << std::endl;
        flag = false;
    }
    return obj;
}

Vector3d obj_from_enu2wgs84(VehicleWgs84 &base_wgs84, Vector3d &obj_enu)
{
    Vector3d obj_wgs84;
    ENUConverter converter(base_wgs84.longitude, base_wgs84.latitude);
    converter.Reverse(obj_enu.x, obj_enu.y, obj_wgs84.x, obj_wgs84.y);

    std::cout << std::fixed << std::setprecision(7);
    std::cout << "obj wgs84:(" << obj_wgs84.x << ", " << obj_wgs84.y << ")"
              << std::endl;
    std::cout << "--------" << std::endl;
    return obj_wgs84;
}

Vector3d locate_fusion(Vector3d &obj1, Vector3d &obj2, double alpha)
{
    Vector3d res;
    res.x = obj1.x * alpha + obj2.x * (1 - alpha);
    res.y = obj1.y * alpha + obj2.y * (1 - alpha);
    return res;
}